function f = f_locatorpose (X, marker, model, corr)
%F = F_LOCATORPOSE (X, MARKER, MODEL, CORR)
%   Berechnet die Minimierungsfunktion zur Ausgleichung der Lokatorpose.
%   X:  Vektor der zu minimierenden Parameter: X = [tx, ty, tz, w, p, k];
%   marker: gemessene Markerpositionen: marker{i}.x,y,z
%   model:  Markerpositionen im Lokatormodell: model{i].x,y,z
%   corr:   Korrespondenzen zwischen den Markern im Modell und den
%   gemessenen Markern: j = corr(i) liefert den Index j des dem
%   Modellmarker i zugeodneten gemessenen Markers.
%
%   See also jacobian_locatorpose, locatorPose.

tx = X(1);
ty = X(2);
tz = X(3);
w = X(4);
p = X(5);
k = X(6);

i = 1;
for j = 1:4
    x = marker{corr(j)}.x;
    y = marker{corr(j)}.y;
    z = marker{corr(j)}.z;
    x0 = model{j}.x;
    y0 = model{j}.y;
    z0 = model{j}.z;
    f(i,1)   = 1*(x - tx -cos(p)*cos(k)*x0+cos(p)*sin(k)*y0-sin(p)*z0);
    f(i+1,1) = 1*(y-ty-(sin(w)*sin(p)*cos(k)+cos(w)*sin(k))*x0-(-sin(w)*sin(p)*sin(k)+cos(w)*cos(k))*y0+sin(w)*cos(p)*z0);
    f(i+2,1) = 1*(z-tz-(-cos(w)*sin(p)*cos(k)+sin(w)*sin(k))*x0-(cos(w)*sin(p)*sin(k)+sin(w)*cos(k))*y0-cos(w)*cos(p)*z0);
    i = i+3;
end
        



